#include "../inc/sai_adapter.h"

StandardClient *sai_adapter::bm_bridge_client_ptr;
SimplePreLAGClient *sai_adapter::bm_bridge_client_mc_ptr;
StandardClient *sai_adapter::bm_router_client_ptr;
StandardClient *sai_adapter::bm_client_ptr;
sai_id_map_t *sai_adapter::sai_id_map_ptr;
Switch_metadata *sai_adapter::switch_metadata_ptr;
std::vector<sai_object_id_t> *sai_adapter::switch_list_ptr;
std::shared_ptr<spdlog::logger> *sai_adapter::logger;
bool sai_adapter::pcap_loop_started;
std::mutex sai_adapter::m;
std::condition_variable sai_adapter::cv;
hostif_trap_id_table_t sai_adapter::hostif_trap_id_table;
pcap_fd_t sai_adapter::cpu_port[2];
int sai_adapter::sniff_pipe_fd[2];
int sai_adapter::nl_fd;
std::vector<netdev_fd_t> sai_adapter::active_netdevs;
adapter_packet_handler_fn sai_adapter::wildcard_entry;
std::mutex sai_adapter::input_mutex_;
std::condition_variable sai_adapter::output_cv_;
std::mutex sai_adapter::output_mutex_;
std::condition_variable sai_adapter::input_cv_;
std::queue<std::string> sai_adapter::input_queue_;
std::queue<std::string> sai_adapter::output_queue_;
int server_fd;

sai_adapter::sai_adapter()
    : //  constructor pre initializations
      //socket(new TSocket("localhost", bm_port)),
      //transport(new TBufferedTransport(socket)),
      //bprotocol(new TBinaryProtocol(transport)),
      //protocol(new TMultiplexedProtocol(bprotocol, "standard")),
      //bm_client(protocol),
      //mc_protocol(new TMultiplexedProtocol(bprotocol, "simple_pre_lag")),
      //bm_bridge_client_mc(mc_protocol),
      //router_socket(new TSocket("localhost", bm_port_router)),
      //router_transport(new TBufferedTransport(router_socket)),
      //router_bprotocol(new TBinaryProtocol(router_transport)),
      //router_protocol(new TMultiplexedProtocol(router_bprotocol, "standard")),
      //bm_router_client(router_protocol),
      port_(11111),
      num_threads_(4),
      stop_(false){
  //  build socket
  // Create a socket object
  // int server_fd = socket(AF_INET, SOCK_STREAM, 0);
  printf("socket begin\n");
  if ((server_fd = ::socket(AF_INET, SOCK_STREAM, 0)) == -1) {
    std::cerr << "connect failed" << std::endl;
    exit(EXIT_FAILURE);
  }
  printf("server_fd created success\n");
  // Set socket options
  int opt = 1;  
  setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT, &opt, sizeof(opt));
  
  // Create a sockaddr_in object with the desired port number
  struct sockaddr_in address;
  address.sin_family = AF_INET;
  address.sin_addr.s_addr = INADDR_ANY;
  address.sin_port = htons(port_);

  // Bind the socket to the sockaddr_in object
  if (bind(server_fd, (struct sockaddr*)&address, sizeof(address)) < 0) {
    std::cerr << "bind socket failed" << std::endl;
    exit(EXIT_FAILURE);
  }
  std::cout << "SOCKET BUILD SUCCESS" << std::endl;
  // Start listening for connections in a separate thread
  if(listen(server_fd, 3) < 0 ) {
      std::cerr << "Failed to listen" << std::endl;
      exit(EXIT_FAILURE);
  }
  if (read_macs_and_ports_from_file("/home/wanglei/env/sai-p4-ppk/sai_adapter/sai.txt")) {
            std::cout << "File cannnot be opened...\n" << std::endl;
  }

  std::thread listenThread = std::thread([server_fd, this] {
    int rv;
    fd_set master;
	  fd_set readfds;
    int maxfds = server_fd;
    int client_fd;
    msg_buf_t*  mem_cell;
	  FD_ZERO(&master);
	  FD_SET(maxfds, &master);
    while (!stop_) {
      readfds = master;
		  select(maxfds+1, &readfds, 0,0,0);
		  for (int i=0;i<maxfds+1;++i) {
        if (FD_ISSET(i, &readfds)) {
          if (i==server_fd) {
					    if ((client_fd = accept(server_fd, (struct sockaddr *)NULL, NULL)) < 0)
			          break;
					    std::cout << "New Device" << std::endl;
              std::thread recv_thread(&sai_adapter::recv_worker_thread, this);
              std::thread send_thread(&sai_adapter::send_worker_thread, this, client_fd);
              recv_thread.detach();
              send_thread.detach();
					    FD_SET(client_fd, &master);
					    maxfds = client_fd>maxfds?client_fd:maxfds;
              // set_default_action_smac();
              // set_default_action_dmac();
              // for (int i=0;i<=mac_count;++i)
              // {
              // printf("Filling tables smac/dmac PORT: %d MAC: %02x:%02x:%02x:%02x:%02x:%02x\n", portmap[i], macs[i][0],macs[i][1],macs[i][2],macs[i][3],macs[i][4],macs[i][5]);
              // fill_dmac_table(portmap[i], macs[i]);
              // fill_smac_table(portmap[i], macs[i]);
              // }
              // notify_ppk();
				} else {
              std::cout << "Received message\n" << std::endl;
					    mem_cell = (msg_buf_t*)malloc(sizeof(msg_buf_t));
					    // mem_cell->length = 4096;
              mem_cell->length = 14;
              rv=read_fix(i, mem_cell->data, mem_cell->length);
              // if ((rv=read_fix(i, mem_cell->data, mem_cell->length)) > 0)
              std::cout << rv << std::endl;
              if(rv >0)
					    // if ((rv=read_p4_msg(i, mem_cell->data, mem_cell->length)) > 0)
                  {
                    std::cout << "Received message is " << mem_cell->data << std::endl;
                    std::unique_lock<std::mutex> lock(sai_adapter::input_mutex_);
                    sai_adapter::input_queue_.push(std::string(mem_cell->data, rv));
                    sai_adapter::input_cv_.notify_one();
		              } else if (rv==0) {
                    std::cout << "Received message length is 0x0000" << mem_cell->data << std::endl;
						        FD_CLR(i, &master);
						        close(i);
						        free(mem_cell);
						        break;
					        }	
				        }
        }
      }
    }
  });
  listenThread.detach();
  // logger
  logger_o = spdlog::get("logger");
  if (logger_o == 0) {
    logger_o = spdlog::basic_logger_mt("logger", "logs/log.txt");
    logger_o->flush_on(spdlog::level::info);   // make err
    spdlog::set_pattern("[thread %t] [%l] %v "); // add %T for time
  }
  logger = &logger_o;
  
  // start P4 link
  switch_list_ptr = &switch_list;
  switch_metadata_ptr = &switch_metadata;
  switch_metadata.hw_port_list.list = list;
  switch_metadata.hw_port_list.count = 8;
  //bm_client_ptr = &bm_client;
  bm_bridge_client_ptr = nullptr;
  bm_router_client_ptr = nullptr;
  bm_bridge_client_mc_ptr = nullptr;
  sai_id_map_ptr = &sai_id_map;
  wildcard_entry = NULL;
  //transport->open();
  //router_transport->open();

  // api set
  switch_api.create_switch = &sai_adapter::create_switch;
  switch_api.remove_switch = &sai_adapter::remove_switch;
  switch_api.get_switch_attribute = &sai_adapter::get_switch_attribute;
  switch_api.set_switch_attribute = &sai_adapter::set_switch_attribute;

  port_api.create_port = &sai_adapter::create_port;
  port_api.remove_port = &sai_adapter::remove_port;
  port_api.set_port_attribute = &sai_adapter::set_port_attribute;
  port_api.get_port_attribute = &sai_adapter::get_port_attribute;
  port_api.get_port_stats = &sai_adapter::get_port_stats;

  bridge_api.create_bridge = &sai_adapter::create_bridge;
  bridge_api.remove_bridge = &sai_adapter::remove_bridge;
  bridge_api.get_bridge_attribute = &sai_adapter::get_bridge_attribute;
  bridge_api.set_bridge_attribute = &sai_adapter::set_bridge_attribute;
  bridge_api.create_bridge_port = &sai_adapter::create_bridge_port;
  bridge_api.remove_bridge_port = &sai_adapter::remove_bridge_port;
  bridge_api.get_bridge_port_attribute =
      &sai_adapter::get_bridge_port_attribute;
  bridge_api.set_bridge_port_attribute =
      &sai_adapter::set_bridge_port_attribute;

  fdb_api.create_fdb_entry = &sai_adapter::create_fdb_entry;
  fdb_api.remove_fdb_entry = &sai_adapter::remove_fdb_entry;

  vlan_api.create_vlan = &sai_adapter::create_vlan;
  vlan_api.remove_vlan = &sai_adapter::remove_vlan;
  vlan_api.set_vlan_attribute = &sai_adapter::set_vlan_attribute;
  vlan_api.get_vlan_attribute = &sai_adapter::get_vlan_attribute;
  vlan_api.create_vlan_member = &sai_adapter::create_vlan_member;
  vlan_api.remove_vlan_member = &sai_adapter::remove_vlan_member;
  vlan_api.set_vlan_member_attribute = &sai_adapter::set_vlan_member_attribute;
  vlan_api.get_vlan_member_attribute = &sai_adapter::get_vlan_member_attribute;
  vlan_api.get_vlan_stats = &sai_adapter::get_vlan_stats;
  vlan_api.clear_vlan_stats = &sai_adapter::clear_vlan_stats;

  lag_api.create_lag = &sai_adapter::create_lag;
  lag_api.remove_lag = &sai_adapter::remove_lag;
  lag_api.create_lag_member = &sai_adapter::create_lag_member;
  lag_api.remove_lag_member = &sai_adapter::remove_lag_member;
  lag_api.get_lag_member_attribute = &sai_adapter::get_lag_member_attribute;

  hostif_api.create_hostif = &sai_adapter::create_hostif;
  hostif_api.remove_hostif = &sai_adapter::remove_hostif;
  hostif_api.set_hostif_attribute = &sai_adapter::set_hostif_attribute;
  hostif_api.get_hostif_attribute = &sai_adapter::get_hostif_attribute;

  hostif_api.create_hostif_table_entry = &sai_adapter::create_hostif_table_entry;
  hostif_api.remove_hostif_table_entry = &sai_adapter::remove_hostif_table_entry;
  hostif_api.create_hostif_trap_group = &sai_adapter::create_hostif_trap_group;
  hostif_api.remove_hostif_trap_group = &sai_adapter::remove_hostif_trap_group;
  hostif_api.set_hostif_trap_group_attribute = &sai_adapter::set_hostif_trap_group_attribute;  
  hostif_api.get_hostif_trap_group_attribute = &sai_adapter::get_hostif_trap_group_attribute;
  hostif_api.create_hostif_trap = &sai_adapter::create_hostif_trap;
  hostif_api.remove_hostif_trap = &sai_adapter::remove_hostif_trap;

  router_interface_api.create_router_interface = &sai_adapter::create_router_interface;
  router_interface_api.remove_router_interface = &sai_adapter::remove_router_interface;
  router_interface_api.set_router_interface_attribute = &sai_adapter::set_router_interface_attribute;
  router_interface_api.get_router_interface_attribute = &sai_adapter::get_router_interface_attribute;
  
  virtual_router_api.create_virtual_router = &sai_adapter::create_virtual_router;
  virtual_router_api.remove_virtual_router = &sai_adapter::remove_virtual_router;
  virtual_router_api.set_virtual_router_attribute = &sai_adapter::set_virtual_router_attribute;
  virtual_router_api.get_virtual_router_attribute = &sai_adapter::get_virtual_router_attribute;
  
  neighbor_api.create_neighbor_entry = &sai_adapter::create_neighbor_entry;
  neighbor_api.remove_neighbor_entry = &sai_adapter::remove_neighbor_entry;
  
  next_hop_api.create_next_hop = &sai_adapter::create_next_hop;
  next_hop_api.remove_next_hop = &sai_adapter::remove_next_hop;
  
  route_api.create_route_entry = &sai_adapter::create_route_entry;
  route_api.remove_route_entry = &sai_adapter::remove_route_entry;
  
  policer_api.create_policer = &sai_adapter::create_policer;
  policer_api.remove_policer = &sai_adapter::remove_policer;
  policer_api.set_policer_attribute = &sai_adapter::set_policer_attribute;
  policer_api.get_policer_attribute = &sai_adapter::get_policer_attribute;
  policer_api.get_policer_stats = &sai_adapter::get_policer_stats;
  policer_api.clear_policer_stats = &sai_adapter::clear_policer_stats;
  mirror_api.create_mirror_session = &sai_adapter::create_mirror_session;
  mirror_api.remove_mirror_session = &sai_adapter::remove_mirror_session;
  mirror_api.set_mirror_session_attribute = &sai_adapter::set_mirror_session_attribute;
  mirror_api.get_mirror_session_attribute = &sai_adapter::get_mirror_session_attribute;
  stp_api.create_stp = &sai_adapter::create_stp;
  stp_api.remove_stp = &sai_adapter::remove_stp;
  stp_api.set_stp_attribute = &sai_adapter::set_stp_attribute;
  stp_api.get_stp_attribute = &sai_adapter::get_stp_attribute;
  stp_api.create_stp_port = &sai_adapter::create_stp_port;
  stp_api.remove_stp_port = &sai_adapter::remove_stp_port;
  stp_api.set_stp_port_attribute = &sai_adapter::set_stp_port_attribute;
  stp_api.get_stp_port_attribute = &sai_adapter::get_stp_port_attribute;
  stp_api.create_stp_ports = &sai_adapter::create_stp_ports;
  stp_api.remove_stp_ports = &sai_adapter::remove_stp_ports;
  samplepacket_api.create_samplepacket = &sai_adapter::create_samplepacket;
  samplepacket_api.remove_samplepacket = &sai_adapter::remove_samplepacket;
  samplepacket_api.set_samplepacket_attribute = &sai_adapter::set_samplepacket_attribute;
  samplepacket_api.get_samplepacket_attribute = &sai_adapter::get_samplepacket_attribute;

  wred_api.create_wred = &sai_adapter::create_wred;
  wred_api.remove_wred = &sai_adapter::remove_wred;
  wred_api.set_wred_attribute = &sai_adapter::set_wred_attribute;
  wred_api.get_wred_attribute = &sai_adapter::get_wred_attribute;
  qos_map_api.create_qos_map = &sai_adapter::create_qos_map;
  qos_map_api.remove_qos_map = &sai_adapter::remove_qos_map;
  qos_map_api.set_qos_map_attribute = &sai_adapter::set_qos_map_attribute;
  qos_map_api.get_qos_map_attribute = &sai_adapter::get_qos_map_attribute;
  queue_api.create_queue = &sai_adapter::create_queue;
  queue_api.remove_queue = &sai_adapter::remove_queue;
  queue_api.set_queue_attribute = &sai_adapter::set_queue_attribute;
  queue_api.get_queue_attribute = &sai_adapter::get_queue_attribute;
  queue_api.get_queue_stats = &sai_adapter::get_queue_stats;
  queue_api.clear_queue_stats = &sai_adapter::clear_queue_stats;
  scheduler_group_api.create_scheduler_group = &sai_adapter::create_scheduler_group;
  scheduler_group_api.remove_scheduler_group = &sai_adapter::remove_scheduler_group;
  scheduler_group_api.set_scheduler_group_attribute = &sai_adapter::set_scheduler_group_attribute;
  scheduler_group_api.get_scheduler_group_attribute = &sai_adapter::get_scheduler_group_attribute;
  scheduler_api.create_scheduler = &sai_adapter::create_scheduler;
  scheduler_api.remove_scheduler = &sai_adapter::remove_scheduler;
  scheduler_api.set_scheduler_attribute = &sai_adapter::set_scheduler_attribute;
  scheduler_api.get_scheduler_attribute = &sai_adapter::get_scheduler_attribute;
  hash_api.create_hash = &sai_adapter::create_hash;
  hash_api.remove_hash = &sai_adapter::remove_hash;
  hash_api.set_hash_attribute = &sai_adapter::set_hash_attribute;
  hash_api.get_hash_attribute = &sai_adapter::get_hash_attribute;
  udf_api.create_udf = &sai_adapter::create_udf;
  udf_api.remove_udf = &sai_adapter::remove_udf;
  udf_api.set_udf_attribute = &sai_adapter::set_udf_attribute;
  udf_api.get_udf_attribute = &sai_adapter::get_udf_attribute;
  udf_api.create_udf_match = &sai_adapter::create_udf_match;
  udf_api.remove_udf_match = &sai_adapter::remove_udf_match;
  udf_api.set_udf_match_attribute = &sai_adapter::set_udf_match_attribute;
  udf_api.get_udf_match_attribute = &sai_adapter::get_udf_match_attribute;
  udf_api.create_udf_group = &sai_adapter::create_udf_group;
  udf_api.remove_udf_group = &sai_adapter::remove_udf_group;
  udf_api.set_udf_group_attribute = &sai_adapter::set_udf_group_attribute;
  udf_api.get_udf_group_attribute = &sai_adapter::get_udf_group_attribute;
  l2mc_group_api.create_l2mc_group = &sai_adapter::create_l2mc_group;
  l2mc_group_api.remove_l2mc_group = &sai_adapter::remove_l2mc_group;
  l2mc_group_api.set_l2mc_group_attribute = &sai_adapter::set_l2mc_group_attribute;
  l2mc_group_api.get_l2mc_group_attribute = &sai_adapter::get_l2mc_group_attribute;
  l2mc_group_api.create_l2mc_group_member = &sai_adapter::create_l2mc_group_member;
  l2mc_group_api.remove_l2mc_group_member = &sai_adapter::remove_l2mc_group_member;
  l2mc_group_api.set_l2mc_group_member_attribute = &sai_adapter::set_l2mc_group_member_attribute;
  l2mc_group_api.get_l2mc_group_member_attribute = &sai_adapter::get_l2mc_group_member_attribute;
  ipmc_api.create_ipmc_entry = &sai_adapter::create_ipmc_entry;
  ipmc_api.remove_ipmc_entry = &sai_adapter::remove_ipmc_entry;
  ipmc_api.set_ipmc_entry_attribute = &sai_adapter::set_ipmc_entry_attribute;
  ipmc_api.get_ipmc_entry_attribute = &sai_adapter::get_ipmc_entry_attribute;
  mcast_fdb_api.create_mcast_fdb_entry = &sai_adapter::create_mcast_fdb_entry;
  mcast_fdb_api.remove_mcast_fdb_entry = &sai_adapter::remove_mcast_fdb_entry;
  mcast_fdb_api.set_mcast_fdb_entry_attribute = &sai_adapter::set_mcast_fdb_entry_attribute;
  mcast_fdb_api.get_mcast_fdb_entry_attribute = &sai_adapter::get_mcast_fdb_entry_attribute;
  buffer_api.create_buffer_pool = &sai_adapter::create_buffer_pool;
  buffer_api.remove_buffer_pool = &sai_adapter::remove_buffer_pool;
  buffer_api.set_buffer_pool_attribute = &sai_adapter::set_buffer_pool_attribute;
  buffer_api.get_buffer_pool_attribute = &sai_adapter::get_buffer_pool_attribute;
  buffer_api.get_buffer_pool_stats = &sai_adapter::get_buffer_pool_stats;
  buffer_api.clear_buffer_pool_stats = &sai_adapter::clear_buffer_pool_stats;
  buffer_api.create_ingress_priority_group = &sai_adapter::create_ingress_priority_group;
  buffer_api.remove_ingress_priority_group = &sai_adapter::remove_ingress_priority_group;
  buffer_api.set_ingress_priority_group_attribute = &sai_adapter::set_ingress_priority_group_attribute;
  buffer_api.get_ingress_priority_group_attribute = &sai_adapter::get_ingress_priority_group_attribute;
  buffer_api.get_ingress_priority_group_stats = &sai_adapter::get_ingress_priority_group_stats;
  buffer_api.clear_ingress_priority_group_stats = &sai_adapter::clear_ingress_priority_group_stats;
  buffer_api.create_buffer_profile = &sai_adapter::create_buffer_profile;
  buffer_api.remove_buffer_profile = &sai_adapter::remove_buffer_profile;
  buffer_api.set_buffer_profile_attribute = &sai_adapter::set_buffer_profile_attribute;
  buffer_api.get_buffer_profile_attribute = &sai_adapter::get_buffer_profile_attribute;
  acl_api.create_acl_table = &sai_adapter::create_acl_table;
  acl_api.remove_acl_table = &sai_adapter::remove_acl_table;
  acl_api.set_acl_table_attribute = &sai_adapter::set_acl_table_attribute;
  acl_api.get_acl_table_attribute = &sai_adapter::get_acl_table_attribute;
  acl_api.create_acl_entry = &sai_adapter::create_acl_entry;
  acl_api.remove_acl_entry = &sai_adapter::remove_acl_entry;
  acl_api.set_acl_entry_attribute = &sai_adapter::set_acl_entry_attribute;
  acl_api.get_acl_entry_attribute = &sai_adapter::get_acl_entry_attribute;
  acl_api.create_acl_counter = &sai_adapter::create_acl_counter;
  acl_api.remove_acl_counter = &sai_adapter::remove_acl_counter;
  acl_api.set_acl_counter_attribute = &sai_adapter::set_acl_counter_attribute;
  acl_api.get_acl_counter_attribute = &sai_adapter::get_acl_counter_attribute;
  acl_api.create_acl_range = &sai_adapter::create_acl_range;
  acl_api.remove_acl_range = &sai_adapter::remove_acl_range;
  acl_api.set_acl_range_attribute = &sai_adapter::set_acl_range_attribute;
  acl_api.get_acl_range_attribute = &sai_adapter::get_acl_range_attribute;
  acl_api.create_acl_table_group = &sai_adapter::create_acl_table_group;
  acl_api.remove_acl_table_group = &sai_adapter::remove_acl_table_group;
  acl_api.set_acl_table_group_attribute = &sai_adapter::set_acl_table_group_attribute;
  acl_api.get_acl_table_group_attribute = &sai_adapter::get_acl_table_group_attribute;
  acl_api.create_acl_table_group_member = &sai_adapter::create_acl_table_group_member;
  acl_api.remove_acl_table_group_member = &sai_adapter::remove_acl_table_group_member;
  acl_api.set_acl_table_group_member_attribute = &sai_adapter::set_acl_table_group_member_attribute;					        
  acl_api.get_acl_table_group_member_attribute = &sai_adapter::get_acl_table_group_member_attribute;


//  startSaiAdapterMain();
  printf("startSaiAdapterMain\n");

}

sai_adapter::~sai_adapter() {
  stop_ = true;
  close(server_fd);
  endSaiAdapterMain();
  //transport->close();
  //router_transport->close();
  (*logger)->info("BM clients closed\n");
}

void sai_adapter::notify_ppk()
{
    char buffer[sizeof(struct p4_header)];
    struct p4_header* h;

    h = create_p4_header(buffer, 0, sizeof(struct p4_header));
    h->type = P4T_CTRL_INITIALIZED;

    netconv_p4_header(h);

    send_p4_msg(buffer, sizeof(struct p4_header));
}

void sai_adapter::recv_worker_thread() {
  //char buffer[1024];
  msg_buf_t*  mem_cell = (msg_buf_t*)malloc(sizeof(msg_buf_t));
	while (!stop_) {
	  std::unique_lock<std::mutex> lock(sai_adapter::input_mutex_);
    while (sai_adapter::input_queue_.empty()) {
			std::cout << "input_queue message empty\n" << std::endl;
			sai_adapter::input_cv_.wait(lock);
		}
    string message = sai_adapter::input_queue_.front();
    for(int i = 0; i < message.size(); ++i) {
      mem_cell->data[i] = message[i];
    }
    mem_cell->data[message.size()] = '\0';
	  sai_adapter::input_queue_.pop();
    std::cout << "input_queue message is" << mem_cell->data << std::endl;
    if (mem_cell==0) continue;
    set_message_handler(mem_cell->data);
	}
  free(mem_cell);
}

void sai_adapter::send_worker_thread(int client_fd) {    
  msg_buf_t* mem_cell = (msg_buf_t*)malloc(sizeof(msg_buf_t));   
	while (!stop_) {		             
		// 从输出队列中取出消息 
		std::unique_lock<std::mutex> lock(sai_adapter::output_mutex_);
		while (sai_adapter::output_queue_.empty()) {
			std::cout << "output_queue message empty" << std::endl;
			sai_adapter::output_cv_.wait(lock);
		}
    string message = sai_adapter::output_queue_.front();
    std::cout << "send worker： " << std::endl;
    std::cout << message << std::endl;
    for(int i = 0; i < message.size(); ++i) {
      mem_cell->data[i] = message[i];
    }
    mem_cell->data[message.size()] = '\0';
    mem_cell->length = message.size() + 1;
		sai_adapter::output_queue_.pop();
    if (mem_cell==0) continue;
		// 发送消息 
    write_p4_msg(client_fd, mem_cell->data, mem_cell->length);
		// send(client_fd, message.c_str(), message.size(), 0);
	}
  free (mem_cell);
}

void sai_adapter::send_message(const std::string& message) {
	std::unique_lock<std::mutex> lock(sai_adapter::output_mutex_);
  sai_adapter::output_queue_.push(message);
	sai_adapter::output_cv_.notify_one();
}

void sai_adapter::set_connection_handler(std::function<void(int)> handler) {        
	connection_handler_ = handler;
}

void sai_adapter::set_message_handler(void* b) {        
    struct p4_header* h = netconv_p4_header(unpack_p4_header((char *)b, 0));
    if (h->type != P4T_DIGEST) {
        printf("Method is not implemented\n");
        return;
    }

    struct p4_digest* d = unpack_p4_digest((char *)b,0);
    if (strcmp(d->field_list_name, "mac_learn_digest")==0) {
        mac_learn_digest(b);
    } else if (strcmp(d->field_list_name, "test_learn_ip")==0) {
        test_learn_ip(b);
    } else {
        printf("Unknown digest received: X%sX\n", d->field_list_name);
    }
}

sai_status_t sai_adapter::sai_api_query(sai_api_t sai_api_id,
                                        void **api_method_table) {
  switch (sai_api_id) {
  case SAI_API_PORT:
    *api_method_table = &port_api;
    break;
  case SAI_API_BRIDGE:
    *api_method_table = &bridge_api;
    break;
  case SAI_API_FDB:
    *api_method_table = &fdb_api;
    break;
  case SAI_API_SWITCH:
    *api_method_table = &switch_api;
    break;
  case SAI_API_VLAN:
    *api_method_table = &vlan_api;
    break;
  case SAI_API_LAG:
    *api_method_table = &lag_api;
    break;
  case SAI_API_HOSTIF:
    *api_method_table = &hostif_api;
    break;
  case SAI_API_VIRTUAL_ROUTER:
    *api_method_table = &virtual_router_api;
    break;
  case SAI_API_ROUTE:
    *api_method_table = &route_api;
    break;
  case SAI_API_NEXT_HOP:
    *api_method_table = &next_hop_api;
    break;
  case SAI_API_NEXT_HOP_GROUP:
    *api_method_table = &next_hop_group_api;
    break;
  case SAI_API_ROUTER_INTERFACE:
    *api_method_table = &router_interface_api;
    break;
  case SAI_API_NEIGHBOR:
    *api_method_table = &neighbor_api;
    break;
  case SAI_API_ACL:
    *api_method_table = &acl_api;
    break;
  case SAI_API_MIRROR:
    *api_method_table = &mirror_api;
    break;
  case SAI_API_SAMPLEPACKET:
    *api_method_table = &samplepacket_api;
    break;
  case SAI_API_STP:
    *api_method_table = &stp_api;
    break;
  case SAI_API_POLICER:
    *api_method_table = &policer_api;
    break;
  case SAI_API_WRED:
    *api_method_table = &wred_api;
    break;
  case SAI_API_QOS_MAP:
    *api_method_table = &qos_map_api;
    break;
  case SAI_API_QUEUE:
    *api_method_table = &queue_api;
    break;
  case SAI_API_SCHEDULER:
    *api_method_table = &scheduler_api;
    break;
  case SAI_API_SCHEDULER_GROUP:
    *api_method_table = &scheduler_group_api;
    break;
  case SAI_API_BUFFER:
    *api_method_table = &buffer_api;
    break;
  case SAI_API_HASH:
    *api_method_table = &hash_api;
    break;
  case SAI_API_UDF:
    *api_method_table = &udf_api;
    break;
  case SAI_API_TUNNEL:
    *api_method_table = &tunnel_api;
    break;
  case SAI_API_L2MC:
    *api_method_table = &l2mc_api;
    break;
  case SAI_API_IPMC:
    *api_method_table = &ipmc_api;
    break;
  case SAI_API_RPF_GROUP:
    *api_method_table = &rpf_group_api;
    break;
  case SAI_API_L2MC_GROUP:
    *api_method_table = &l2mc_group_api;
    break;
  case SAI_API_IPMC_GROUP:
    *api_method_table = &ipmc_group_api;
    break;
  case SAI_API_MCAST_FDB:
    *api_method_table = &mcast_fdb_api;
    break;
  default:
    (*logger)->info("api requested was %d, while sai_api_port is %d\n",
                    sai_api_id, SAI_API_PORT);
    return SAI_STATUS_FAILURE;
  }
  return SAI_STATUS_SUCCESS;
}

sai_object_type_t sai_adapter::_sai_object_type_query(sai_object_id_t sai_object_id) {
	if (switch_metadata_ptr->switch_id == sai_object_id) {
		return SAI_OBJECT_TYPE_SWITCH;
	}
	if (switch_metadata_ptr->cpu_port_id == sai_object_id) {
		return SAI_OBJECT_TYPE_PORT;
	}
	if (switch_metadata_ptr->ports.find(sai_object_id) != switch_metadata_ptr->ports.end()) {
		return SAI_OBJECT_TYPE_PORT;
	}
	if (switch_metadata_ptr->bridge_ports.find(sai_object_id) != switch_metadata_ptr->bridge_ports.end()) {
		return SAI_OBJECT_TYPE_BRIDGE_PORT;
	}
	if (switch_metadata_ptr->bridges.find(sai_object_id) != switch_metadata_ptr->bridges.end()) {
		return SAI_OBJECT_TYPE_BRIDGE;
	}
	if (switch_metadata_ptr->vlans.find(sai_object_id) != switch_metadata_ptr->vlans.end()) {
		return SAI_OBJECT_TYPE_VLAN;
	}
	if (switch_metadata_ptr->vlan_members.find(sai_object_id) != switch_metadata_ptr->vlan_members.end()) {
		return SAI_OBJECT_TYPE_VLAN_MEMBER;
	}
	if (switch_metadata_ptr->lags.find(sai_object_id) != switch_metadata_ptr->lags.end()) {
		return SAI_OBJECT_TYPE_LAG;
	}
	if (switch_metadata_ptr->lag_members.find(sai_object_id) != switch_metadata_ptr->lag_members.end()) {
		return SAI_OBJECT_TYPE_LAG_MEMBER;
	}
	if (switch_metadata_ptr->hostifs.find(sai_object_id) != switch_metadata_ptr->hostifs.end()) {
		return SAI_OBJECT_TYPE_HOSTIF;
	}
	if (switch_metadata_ptr->hostif_table_entries.find(sai_object_id) != switch_metadata_ptr->hostif_table_entries.end()) {
		return SAI_OBJECT_TYPE_HOSTIF_TABLE_ENTRY;
	}
	if (switch_metadata_ptr->hostif_traps.find(sai_object_id) != switch_metadata_ptr->hostif_traps.end()) {
		return SAI_OBJECT_TYPE_HOSTIF_TRAP;
	}
       	if (switch_metadata_ptr->hostif_trap_groups.find(sai_object_id) != switch_metadata_ptr->hostif_trap_groups.end()) {
  		return SAI_OBJECT_TYPE_HOSTIF_TRAP_GROUP;
	}
    	if (switch_metadata_ptr->vrs.find(sai_object_id) != switch_metadata_ptr->vrs.end()) {
		return SAI_OBJECT_TYPE_VIRTUAL_ROUTER;
	}
  	if (switch_metadata_ptr->rifs.find(sai_object_id) != switch_metadata_ptr->rifs.end()) {
		return SAI_OBJECT_TYPE_ROUTER_INTERFACE;
	}
	if (switch_metadata_ptr->nhops.find(sai_object_id) != switch_metadata_ptr->nhops.end()) {
		return SAI_OBJECT_TYPE_NEXT_HOP;
	}
	return SAI_OBJECT_TYPE_NULL;
}
void sai_adapter::internal_init_switch() {
  sai_object_id_t switch_id2;
  (*logger)->info("Switch init with default configurations");
  switch_api.create_switch(&switch_id2, 0, NULL);
  (*logger)->info("Switch init with default configurations done");
  return;
}
void sai_adapter::startSaiAdapterMain() {
  //internal_init_switch();
  pcap_loop_started = false;
  SaiAdapterThread = std::thread(&sai_adapter::SaiAdapterMain, this);
  std::unique_lock<std::mutex> lk(m);
  cv.wait(lk,[]{return pcap_loop_started;});
  //std::this_thread::sleep_for(std::chrono::milliseconds(500)); // TODO consider later release of lock
  (*logger)->info("Sniffer initialization done");
}

void sai_adapter::endSaiAdapterMain() {
  //pcap_breakloop(adapter_pcap);
  //pcap_close(adapter_pcap);
  (*logger)->info("endSaiAdapterMain");
  write(sniff_pipe_fd[1],"c",1);
  SaiAdapterThread.join();
}

void sai_adapter::SaiAdapterMain() {
  //(*logger)->info("SAI Adapter Thread Started");
  // Change to sai_adapter network namespace (hostif_net)
  //int fd = open("/var/run/netns/hostif_net",
  //              O_RDONLY); /* Get descriptor for namespace */
  //if (fd == -1) {
  //  (*logger)->error("open netns fd failed");
  //  release_pcap_lock();
  //  return;
  //}
  //if (setns(fd, 0) == -1) { /* Join that namespace */
  //  (*logger)->error("setns failed");
  //  release_pcap_lock();
  //  return;
  //}

  (*logger)->info("SAI Adapter Thread Started");
  PacketSniffer();
  (*logger)->info("SAI Adapter Thread Ended");
}

int sai_adapter::read_macs_and_ports_from_file(char *filename) {
    FILE *f;
    char line[100];
    int values[6];
    int port;
    int i;

    f = fopen(filename, "r");
    if (f == NULL) return -1;

    while (fgets(line, sizeof(line), f)) {
        line[strlen(line)-1] = '\0';

        if (7 == sscanf(line, "%x:%x:%x:%x:%x:%x %d",
                        &values[0], &values[1], &values[2],
                        &values[3], &values[4], &values[5], &port) )
        {
            if (mac_count==MAX_MACS-1)
            {
                printf("Too many entries...\n");
                break;
            }

            ++mac_count;
            for( i = 0; i < 6; ++i )
                    macs[mac_count][i] = (uint8_t) values[i];
            portmap[mac_count] = (uint8_t) port;

        } else {
            printf("Wrong format error in line %d : %s\n", mac_count+2, line);
            fclose(f);
            return -1;
        }

    }

    fclose(f);
    return 0;
}

int sai_adapter::read_fix(int sock, char* buffer, int nbytes)
{
	int rbytes = 0;
	int rval = 0;
	while (rbytes<nbytes)
	{
		rval = read(sock, buffer + rbytes, nbytes-rbytes);
		if (rval<=0) return rval;
		rbytes += rval;
	}
	return rbytes;
}

int sai_adapter::write_fix(int sock, char* buffer, int nbytes)
{
        int wbytes = 0;
        int wval = 0;
        while (wbytes<nbytes)
        {
                wval = write(sock, buffer + wbytes, nbytes-wbytes);
                if (wval<0) return wval;
                wbytes += wval;
        }

        return wbytes;
}

int sai_adapter::read_p4_msg(int sock, char* buffer, int length)
{
	int rval;
	int msglen;
	if (length<sizeof(struct p4_header)) return -1;

	if ((rval=read_fix(sock, buffer, sizeof(struct p4_header)))<=0)
		return rval;
	
	msglen = ntohs(((struct p4_header*)buffer)->length);

	if (msglen>length)
		return -100;

	if (msglen == sizeof(struct p4_header))
		return msglen;

	if ((rval=read_fix(sock, buffer + sizeof(struct p4_header), msglen-sizeof(struct p4_header)))<=0)
		return rval;
	return msglen;
}

int sai_adapter::send_p4_msg(char* buffer, int length)
{
	msg_buf_t* mem_cell;
	if (length>4096)
		return 0;

	mem_cell = (msg_buf_t*)malloc(sizeof(msg_buf_t));

	memcpy(mem_cell->data, buffer, length);
	mem_cell->length = length;

  std::cout << "put message to output_queue" << std::endl;
  std::unique_lock<std::mutex> lock(sai_adapter::output_mutex_);
  std::cout << "message is: " << std::endl;
  std::cout << std::string(mem_cell->data, length) << std::endl;
  sai_adapter::output_queue_.push(std::string(mem_cell->data, length));
  sai_adapter::output_cv_.notify_one();
	return 0;
}

int sai_adapter::write_p4_msg(int sock, char* buffer, int length)
{
    int msglen;
    if (length<sizeof(struct p4_header)) return -1;

	msglen = ntohs(((struct p4_header*)buffer)->length);

	if (msglen>length) return -1;
  std::cout << "ready for write_fix\n" << std::endl;
	return write_fix(sock, buffer, msglen);
}

void sai_adapter::set_default_action_smac()
{
    char buffer[2048];
    struct p4_header* h;
    struct p4_set_default_action* sda;
    struct p4_action* a;

    printf("Generate set_default_action message for table smac\n");

    h = create_p4_header(buffer, 0, sizeof(buffer));

    sda = create_p4_set_default_action(buffer,0,sizeof(buffer));
    strcpy(sda->table_name, "smac_0");

    a = &(sda->action);
    strcpy(a->description.name, "mac_learn");

    netconv_p4_header(h);
    netconv_p4_set_default_action(sda);
    netconv_p4_action(a);

    send_p4_msg(buffer, sizeof(buffer));
}

void sai_adapter::set_default_action_dmac()
{
    char buffer[2048];
    struct p4_header* h;
    struct p4_set_default_action* sda;
    struct p4_action* a;

    printf("Generate set_default_action message for table dmac\n");

    h = create_p4_header(buffer, 0, sizeof(buffer));

    sda = create_p4_set_default_action(buffer,0,sizeof(buffer));
    strcpy(sda->table_name, "dmac_0");

    a = &(sda->action);
    strcpy(a->description.name, "bcast");

    netconv_p4_header(h);
    netconv_p4_set_default_action(sda);
    netconv_p4_action(a);

    send_p4_msg(buffer, sizeof(buffer));
}

void sai_adapter::fill_smac_table(uint8_t port, uint8_t mac[6])
{
    char buffer[2048];
    struct p4_header* h;
    struct p4_add_table_entry* te;
    struct p4_action* a;
    // struct p4_action_parameter* ap;
    // struct p4_field_match_header* fmh;
    struct p4_field_match_exact* exact;

    h = create_p4_header(buffer, 0, 2048);
    te = create_p4_add_table_entry(buffer,0,2048);
    strcpy(te->table_name, "smac_0");

    exact = add_p4_field_match_exact(te, 2048);
    strcpy(exact->header.name, "ethernet.srcAddr");
    memcpy(exact->bitmap, mac, 6);
    exact->length = 6*8+0;

    a = add_p4_action(h, 2048);
    strcpy(a->description.name, "_nop");

    netconv_p4_header(h);
    netconv_p4_add_table_entry(te);
    netconv_p4_field_match_exact(exact);
    netconv_p4_action(a);

    send_p4_msg(buffer, 2048);
}

void sai_adapter::fill_dmac_table(uint8_t port, uint8_t mac[6])
{
    char buffer[2048];
    struct p4_header* h;
    struct p4_add_table_entry* te;
    struct p4_action* a;
    struct p4_action_parameter* ap;
    struct p4_field_match_exact* exact;

    h = create_p4_header(buffer, 0, 2048);
    te = create_p4_add_table_entry(buffer,0,2048);
    strcpy(te->table_name, "dmac_0");

    exact = add_p4_field_match_exact(te, 2048);
    strcpy(exact->header.name, "ethernet.dstAddr");
    memcpy(exact->bitmap, mac, 6);
    exact->length = 6*8+0;

    a = add_p4_action(h, 2048);
    strcpy(a->description.name, "forward");

    ap = add_p4_action_parameter(h, a, 2048);
    strcpy(ap->name, "port");
    memcpy(ap->bitmap, &port, 1);
    ap->length = 1*8+0;

    netconv_p4_header(h);
    netconv_p4_add_table_entry(te);
    netconv_p4_field_match_exact(exact);
    netconv_p4_action(a);
    netconv_p4_action_parameter(ap);

    send_p4_msg(buffer, 2048);
}

void sai_adapter::mac_learn_digest(void* b) {
    uint8_t mac[6];
    uint8_t port[2];
    uint16_t offset=0;
    offset = sizeof(struct p4_digest);
    struct p4_digest_field* df = netconv_p4_digest_field(unpack_p4_digest_field((char *)b, offset));
    memcpy(mac, df->value, 6);
    offset += sizeof(struct p4_digest_field);
    df = netconv_p4_digest_field(unpack_p4_digest_field((char *)b, offset));
    memcpy(port, df->value, 2);

    uint8_t p = port[0];
    printf("Ctrl: mac_learn_digest PORT: %d MAC: %02x:%02x:%02x:%02x:%02x:%02x\n", p, mac[0],mac[1],mac[2],mac[3],mac[4],mac[5]);
    fill_dmac_table(p, mac);
    fill_smac_table(p, mac);
}

void sai_adapter::test_learn_ip(void* b) {
    uint8_t ip[4];
    uint16_t pr;
    int i1;
    int i2;
    int i3;

    uint16_t offset=0;
    offset = sizeof(struct p4_digest);
    struct p4_digest_field* df = netconv_p4_digest_field(unpack_p4_digest_field((char *)b, offset));
    memcpy(ip, df->value, 4);
    offset += sizeof(struct p4_digest_field);
    df = netconv_p4_digest_field(unpack_p4_digest_field((char *)b, offset));
    memcpy(&pr, df->value, 2);
    offset += sizeof(struct p4_digest_field);
    df = netconv_p4_digest_field(unpack_p4_digest_field((char *)b, offset));
    memcpy(&i1, df->value, 4);
    offset += sizeof(struct p4_digest_field);
    df = netconv_p4_digest_field(unpack_p4_digest_field((char *)b, offset));
    memcpy(&i2, df->value, 4);
    offset += sizeof(struct p4_digest_field);
    df = netconv_p4_digest_field(unpack_p4_digest_field((char *)b, offset));
    memcpy(&i3, df->value, 4);

    char buffer[2048];
    struct p4_header* h;
    struct p4_add_table_entry* te;
    struct p4_action* a;
    struct p4_action_parameter *ap1, *ap2, *ap3;
    struct p4_field_match_lpm* lpm;
    h = create_p4_header(buffer, 0, 2048);
    te = create_p4_add_table_entry(buffer,0,2048);
    strcpy(te->table_name, "table1");
    lpm = add_p4_field_match_lpm(te, 2048);
    strcpy(lpm->header.name, "field1");
    memcpy(lpm->bitmap, ip, 4);
    lpm->prefix_length = pr;
    a = add_p4_action(h, 2048);
    strcpy(a->description.name, "korte");
    ap1 = add_p4_action_parameter(h, a, 2048);
    memcpy(ap1->bitmap, &i1, 4);
    ap2 = add_p4_action_parameter(h, a, 2048);
    memcpy(ap2->bitmap, &i2, 4);
    ap3 = add_p4_action_parameter(h, a, 2048);
    memcpy(ap3->bitmap, &i3, 4);
    netconv_p4_header(h);
    netconv_p4_add_table_entry(te);
    netconv_p4_field_match_lpm(lpm);
    netconv_p4_action(a);
    netconv_p4_action_parameter(ap1);
    netconv_p4_action_parameter(ap2);
    netconv_p4_action_parameter(ap3);
    send_p4_msg(buffer, 2048);
}

std::string parse_param(uint64_t param, uint32_t num_of_bytes) {
  std::string my_string = std::string(
      static_cast<char *>(static_cast<void *>(&param)), num_of_bytes);
  std::reverse(my_string.begin(), my_string.end());
  return my_string;
}

BmMatchParam parse_exact_match_param(uint64_t param, uint32_t num_of_bytes) {
  BmMatchParam match_param;
  match_param.type = BmMatchParamType::type::EXACT;
  BmMatchParamExact match_param_exact;
  match_param_exact.key = parse_param(param, num_of_bytes);
  match_param.__set_exact(match_param_exact);
  return match_param;
}

BmMatchParam parse_lpm_param(uint64_t param, uint32_t num_of_bytes, uint32_t prefix_length) {
  BmMatchParam match_param;
  match_param.type = BmMatchParamType::type::LPM;
  BmMatchParamLPM match_param_lpm;
  match_param_lpm.__set_key(parse_param(param, num_of_bytes));
  match_param_lpm.__set_prefix_length(prefix_length);
  match_param.__set_lpm(match_param_lpm);
  return match_param;
}

BmMatchParam parse_ternary_param(uint64_t param, uint32_t num_of_bytes, uint64_t mask) {
  BmMatchParam match_param;
  match_param.type = BmMatchParamType::type::TERNARY;
  BmMatchParamTernary match_param_ternary;
  match_param_ternary.__set_key(parse_param(param, num_of_bytes));
  match_param_ternary.__set_mask(parse_param(mask, num_of_bytes));
  match_param.__set_ternary(match_param_ternary);
  return match_param;
}

BmMatchParam parse_valid_match_param(bool param) {
  BmMatchParam match_param;
  match_param.type = BmMatchParamType::type::VALID;
  BmMatchParamValid match_param_valid;
  match_param_valid.key = param;
  match_param.__set_valid(match_param_valid);
  return match_param;
}
/*
uint64_t parse_mac_64(uint8_t const mac_8[6]) {
  uint64_t mac_64 = 0;
  memcpy(&mac_64, mac_8, 6);
  return mac_64;
}*/


uint64_t parse_mac_64(uint8_t const mac_8[6]) {
  uint64_t mac_64 = 0;
  uint8_t mac_8_reversed[6];
  mac_8_reversed[0] = mac_8[5];
  mac_8_reversed[1] = mac_8[4];
  mac_8_reversed[2] = mac_8[3];
  mac_8_reversed[3] = mac_8[2];
  mac_8_reversed[4] = mac_8[1];
  mac_8_reversed[5] = mac_8[0];
  memcpy(&mac_64, mac_8_reversed, 6);
  return mac_64;
}
 
